﻿using System;
using System.Diagnostics;
using System.Collections.Generic;
using System.IO;
using System.Drawing;
using System.Windows.Forms;
//using OpenGL;

namespace System
{
	public class MathRegister
	{
		public List<double[]> Values;
		public List<double> Times;
		public double Length;
		public MathRegister(double len)
		{
			Values = new List<double[]>();
			Times = new List<double>();
			Length = len;
		}

		public void Process(double dt, double[] val)
		{
			Values.Insert(0, val);
			Times.Insert(0, -dt);
			while (Times[0] + dt > Length)
			{
				Times.RemoveAt(0);
				Values.RemoveAt(0);
			}
			for (int i = 0; i < Times.Count; i++)
			{
				Times[i] += dt;
			}
		}
		public void DrawGraphics(Graphics G)
		{
			List<PointF> L;
			for (int N = 0; N < Values[0].Length; N++)
			{
				L = new List<PointF>();
				for (int i = 0; i < Times.Count; i++)
				{
					V3D P = new V3D(Length - Times[i], Values[i][N], 0);
					L.Add(new PointF((float)P.X, (float)P.Y));
				}
				G.DrawLines(Pens.Red, L.ToArray());
			}
		}
	}
	public class PT2
	{
		public double T1;
		public double T2;
		public double Value;
		MathRegister Register;
		public PT2(double tparam1, double tparam2, double reglen)
		{
			T1 = tparam1;
			T2 = tparam2;
			Value = 0;
			Register = new MathRegister(reglen);
		}
		public void Process(double dt, double A)
		{
			PT1 I1 = new PT1(T1, 1, 1);
			PT1 I2 = new PT1(T2, 1, 1);
			I1.Value = Value;
			I2.Value = Value;
			Value = I2.Process(dt, I1.Process(dt, A));
			//double e = A - Value;
			//double delta = e / T;
			//Value += delta * dt;
			//Value = Math.Max(MinCap, Math.Min(MaxCap, Value));
			Register.Process(dt, new double[] { A, Value });
		}
		public void DrawGraphics(Graphics G)
		{
			Register.DrawGraphics(G);
		}
	}
	public class PID
	{
		public double P_K;
		public double I_T;
		public double D_T;
		public double Value;
		public double E0, E1, E2;
		public MathRegister Register;
		public PID(double pk, double dt, double it)
		{
			P_K = pk;
			I_T = it;
			D_T = dt;
			Value = E0 = E1 = E2 = 0;
			Register = new MathRegister(500);
		}
		public void Process(double dt, double A)
		{
			E2 = E1;
			E1 = E0;
			E0 = Value - A;
			Value = Value + P_K * ((1 + dt / I_T + D_T / dt) * E0 + (-1 - 2 * D_T / dt) * E1 + D_T / dt * E2);

			//double e = A - Value;
			//double delta = e / T;
			//Value += delta * dt;
			//Value = Math.Max(MinCap, Math.Min(MaxCap, Value));
			Register.Process(dt, new double[] { A, Value });
		}
		public void DrawGraphics(Graphics G)
		{
			Register.DrawGraphics(G);
		}
	}
}